﻿using UnityEngine;
using System.Collections;

public class AStarPathFinding {

    public static int HeuristicEstimateCost(ASNode sNode,ASNode eNode)
    {
        Vector3 cost = sNode.position - eNode.position;
        return (int)cost.magnitude;
    }
    
    public static ArrayList FindPath(Vector3 startPosition,Vector3 endPosition,int map_width,int map_height,int grid_size)
    {
        PriorityQueue<ASNode> openList = new PriorityQueue<ASNode>(10000,false); //10000表示使用初始化为10000大小的优先队列,false 表示使用的最小优先队列
        //ArrayList closeList = new ArrayList();
        GridManager gridMgr = GridManager.getInstace();
        gridMgr.Initialiaze(Vector3.zero,map_width,map_height,grid_size);

        ASNode goalNode = gridMgr.getNodeByPosition(endPosition);
        ASNode startNode = gridMgr.getNodeByPosition(startPosition);
        openList.Insert(startNode);
        startNode.inOpenLs = 1;

        ASNode curNode = null;
        while (openList.Length != 0)
        {
            curNode = openList.Extract_Minimum_Maximum();
            curNode.inOpenLs = 0;

            if (gridMgr.ANodeEqualToBNode(curNode, goalNode))
            {
                return CalculatePath(goalNode);
            }

            //add all neighbours
            ArrayList neighbours = gridMgr.getNeighbourNodes(curNode);
            foreach (ASNode item in neighbours)
            {
                if (item.inOpenLs != 2)
                {
                    int newGCost = HeuristicEstimateCost(curNode, item) + curNode.GCost;
                    
                    if (item.inOpenLs == 0)
                    {
                        item.GCost = newGCost;
                        item.HCost = HeuristicEstimateCost(item, goalNode);
                        item.parent = curNode;
                        item.inOpenLs = 1;
                        openList.Insert(item);
                    }
                    else
                    {
                        if (item.GCost < newGCost) { }
                        else
                        {
                            item.GCost = newGCost;
                            item.parent = curNode;
                            openList.BuildMinMax_Heap();//该优先队列中不支持记录当前节点的下标,所以改变优先级之后无法直接操作改元素,所以排列所有 < O(n) 时间.
                        }
                    }
                    
                }
            }

            //closeList.Add(curNode);
            curNode.inOpenLs = 2;
        }
        return null;
    }

    public static ArrayList CalculatePath(ASNode endNode)
    {
        ArrayList tempArray = new ArrayList();
        tempArray.Add(endNode);
        while (endNode.parent != null)
        {
            endNode = endNode.parent;
            tempArray.Add(endNode);
        }
        tempArray.Reverse();
        return tempArray;
    }
    
}
